#include "px4_flight_handler.h"


#define SBUF_SIZE 					512



using namespace std;
using namespace mavsdk;



int64_t getTimeMillis()
{
	int64_t timeMs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	return timeMs;
}

void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}

double wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}

double wrap180(const double angle)
{
    auto res = wrap360(angle);
    if (res > 180)
	{
        res -= 360;
    }
    return res;
}


px4FlightHandler::px4FlightHandler():
	m_logFp(NULL),
	m_getLogDirFlag(false),
	m_lastLogNum(0)
{

}

px4FlightHandler::~px4FlightHandler()
{
	if(NULL != m_logFp)
	{
		fflush(m_logFp);
		fclose(m_logFp);
		m_logFp = NULL;
	}
}


bool px4FlightHandler::startUpSystemUdp(const std::string& local_bind_ip, const int local_port)
{
	logInit();

	ConnectionResult connection_result;
	connection_result = m_mavsdk.add_udp_connection(local_bind_ip, local_port);

	if (connection_result != ConnectionResult::SUCCESS)
	{
		cout << "Connection failed: " << connection_result_str(connection_result) << std::endl;
		return false;
    }

	getMavsdkHandler();

	return true;
}

bool px4FlightHandler::startUpSystemUart(const std::string& dev_path, const int baudrate)
{
	logInit();

	ConnectionResult connection_result;
	connection_result = m_mavsdk.add_serial_connection(dev_path, baudrate);

	if (connection_result != ConnectionResult::SUCCESS)
	{
		cout << "Connection failed: " << connection_result_str(connection_result) << std::endl;
		return false;
    }

	getMavsdkHandler();

	return true;
}


void px4FlightHandler::getMavsdkHandler()
{
	System& system = m_mavsdk.system();
    m_telemetry = std::make_shared<mavsdk::Telemetry>(system);
    m_action = std::make_shared<mavsdk::Action>(system);
    m_offboard = std::make_shared<mavsdk::Offboard>(system);
}


void px4FlightHandler::logInit()
{
	bool ret;

	// start px4 control log
	ret = getLogDir();
	if(!ret)
	{
		cout << "getLogDir failed" << endl;
	}
	else
	{
		ret = createLogFile();
		if(!ret)
		{
			cout << "createLogFile failed" << endl;
		}
	}
}


bool px4FlightHandler::getLogDir()
{
	if (0 == access("./px4_control_log", F_OK))
	{
		m_getLogDirFlag = true;
		return true;
	}
	else
	{
		int ret = mkdir("./px4_control_log", 0777);

		if(0 == ret)
		{
			m_getLogDirFlag = true;
			return true;
		}
		else
		{
			m_getLogDirFlag = false;
			return false;
		}
	}

	return false;
}

bool px4FlightHandler::createLogFile()
{
	if(!m_getLogDirFlag)
	{
		return false;
	}

	for(int i=1; i<1000; i++)
	{
		char logName[50];
		memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
		sprintf(logName,"./px4_control_log/px4_control_logs_%d.txt", i);
		m_lastLogNum = i;
		if(0 == access(logName, F_OK))
		{
			continue;
		}
		else
		{
			if(999 == i)
			{
				if(0 == system("rm  ./px4_control_log/*  -rf"))
				{
					m_logFp =  fopen("./px4_control_log/px4_control_logs_1.txt", "ab+");
					if(NULL == m_logFp)
					{
						m_lastLogNum = 0;
						cout << "createlogFile failed1" << endl;
						return false;
					}
					m_lastLogNum = 1;
					return true;
				}
				else
				{
					m_lastLogNum--;
					cout << "rm ./px4_control_log/* -rf failed" << endl;
					m_logFp = NULL;
					return false;
				}
			}
			else
			{
				m_logFp =  fopen(logName, "ab+");
				if(NULL == m_logFp)
				{
					m_lastLogNum--;
					cout << "createLogFile failed2" << endl;
					return false;
				}
				return true;
			}
		}
	}

	return false;
}


bool px4FlightHandler::writeLogFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == m_logFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, m_logFp);
    if (size != n) {
        fclose(m_logFp);
		m_logFp = NULL;
		cout << "writeLogFile fialed" << endl;
        return false;
    }

	return true;
}


void px4FlightHandler::recodeCpuTemperature()
{
	FILE *file = fopen("/sys/class/thermal/thermal_zone0/temp", "r");
	if(NULL != file)
	{
		char buf[10];
		float temp;
		memset(buf, 0, 10);
		size_t len = fread(buf, 1, 10, file);
		if(len > 0)
		{
			temp  = atof(buf);
			writeLogFile("cpu temperature is %f\n", temp / 1000.0f);
		}
		fclose(file);
	}
}


void px4FlightHandler::fflushLogFile()
{
	if(NULL != m_logFp)
	{
		fflush(m_logFp);
	}
}

mavsdk::Telemetry::FlightMode px4FlightHandler::getCurrentFlightMode()
{
	return m_telemetry->flight_mode();
}


mavsdk::Telemetry::LandedState px4FlightHandler::getCurrentFlightStatus()
{
	return m_telemetry->landed_state();
}

mavsdk::Telemetry::EulerAngle px4FlightHandler::getCurrentFlightAttitude()
{
	return m_telemetry->attitude_euler_angle();
}


bool px4FlightHandler::armFlight()
{
	Action::Result ret = m_action->arm();
	if(ret == Action::Result::SUCCESS)
	{
		writeLogFile("armFlight ok\n");
		return true;
	}

	writeLogFile("armFlight error:%d\n", (int)ret);
	return false;
}

bool px4FlightHandler::disarmFlight()
{
	Action::Result ret = m_action->disarm();
	if(ret == Action::Result::SUCCESS)
	{
		writeLogFile("disarmFlight ok\n");
		return true;
	}

	writeLogFile("disarmFlight error:%d\n", (int)ret);
	return false;
}


bool px4FlightHandler::takeoffFlight()
{
	Action::Result ret = m_action->takeoff();
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("takeoffFlight takeoff error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("takeoffFlight ok\n");

	return true;
}


bool px4FlightHandler::takeoffFlight(float relative_altitude_m)
{
	Action::Result ret = m_action->set_takeoff_altitude(relative_altitude_m);
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("takeoffFlight set_takeoff_altitude error:%d\n", (int)ret);
		return false;
	}

	ret = m_action->takeoff();
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("takeoffFlight takeoff error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("takeoffFlight ok\n");

	return true;
}


bool px4FlightHandler::landFlight()
{
	Action::Result ret = m_action->land();
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("landFlight error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("landFlight ok\n");

	return true;
}

bool px4FlightHandler::rtlFlight()
{
	Action::Result ret = m_action->return_to_launch();
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("rtlFlight error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("rtlFlight ok\n");

	return true;
}


bool px4FlightHandler::guidedFlight(double latitude_deg, double longitude_deg, float altitude_amsl_m, float yaw_deg)
{
	Action::Result ret = m_action->goto_location(latitude_deg, longitude_deg, altitude_amsl_m, yaw_deg);
	if(ret != Action::Result::SUCCESS)
	{
		writeLogFile("guidedFlight error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("guidedFlight ok\n");

	return true;
}


bool px4FlightHandler::flightIsInAir()
{
	return m_telemetry->in_air();
}


bool px4FlightHandler::offboardIsActive()
{
	return m_offboard->is_active();
}



bool px4FlightHandler::startOffboardAttitudeRateControl()
{
	// Send it once before starting offboard, otherwise it will be rejected.
    m_offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 1.0f});

	Offboard::Result ret = m_offboard->start();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("startOffboardAttitudeRateControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("startOffboardAttitudeRateControl ok\n");

	return true;
}

void px4FlightHandler::setAttitudeRate(float roll_deg_s, float pitch_deg_s, float yaw_deg_s, float thrust_value)
{
	m_offboard->set_attitude_rate({roll_deg_s, pitch_deg_s, yaw_deg_s, thrust_value});
	writeLogFile("setAttitudeRate roll:%f, pitch:%f, yaw:%f, thrust:%f\n", roll_deg_s, pitch_deg_s, yaw_deg_s, thrust_value);
}

bool px4FlightHandler::stopOffboardAttitudeRateControl()
{
	Offboard::Result ret = m_offboard->stop();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("stopOffboardAttitudeRateControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("stopOffboardAttitudeRateControl ok\n");

	return true;
}


bool px4FlightHandler::startOffboardAttitudeControl()
{
	// Send it once before starting offboard, otherwise it will be rejected.
    m_offboard->set_attitude({0.0f, 0.0f, getCurrentFlightAttitude().yaw_deg, 0.5f});

	Offboard::Result ret = m_offboard->start();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("startOffboardAttitudeControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("startOffboardAttitudeControl ok\n");

	return true;
}

void px4FlightHandler::setAttitude(float roll_deg, float pitch_deg, float yaw_deg, float thrust_value)
{
	m_offboard->set_attitude({roll_deg, pitch_deg, yaw_deg, thrust_value});
	writeLogFile("setAttitude roll:%f, pitch:%f, yaw:%f, thrust:%f\n", roll_deg, pitch_deg, yaw_deg, thrust_value);
}

bool px4FlightHandler::stopOffboardAttitudeControl()
{
	Offboard::Result ret = m_offboard->stop();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("stopOffboardAttitudeControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("stopOffboardAttitudeControl ok\n");

	return true;
}


bool px4FlightHandler::startOffboardPositionControl()
{
	// Send it once before starting offboard, otherwise it will be rejected.
	m_offboard->set_position_ned({0.0f, 0.0f, 0.0f, getCurrentFlightAttitude().yaw_deg});
	Offboard::Result ret = m_offboard->start();

	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("startOffboardPositionControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("startOffboardPositionControl ok\n");

	return true;
}

void px4FlightHandler::setPositionNed(float north_m, float east_m, float down_m, float yaw_deg)
{
	m_offboard->set_position_ned({north_m, east_m, down_m, yaw_deg});
	writeLogFile("setPositionNed north:%f, east:%f, down:%f, yaw:%f\n", north_m, east_m, down_m, yaw_deg);
}

bool px4FlightHandler::stopOffboardPositionControl()
{
	Offboard::Result ret = m_offboard->stop();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("stopOffboardPositionControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("stopOffboardPositionControl ok\n");

	return true;
}


bool px4FlightHandler::startOffboardVelocityNedControl()
{
	// Send it once before starting offboard, otherwise it will be rejected.
	m_offboard->set_velocity_ned({0.0f, 0.0f, 0.0f, getCurrentFlightAttitude().yaw_deg});
	Offboard::Result ret = m_offboard->start();

	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("startOffboardVelocityNedControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("startOffboardVelocityNedControl ok\n");

	return true;
}

void px4FlightHandler::setVelocityNed(float north_m_s, float east_m_s, float down_m_s, float yaw_deg)
{
	m_offboard->set_velocity_ned({north_m_s, east_m_s, down_m_s, yaw_deg});
	writeLogFile("setVelocityNed north:%f, east:%f, down:%f, yaw:%f\n", north_m_s, east_m_s, down_m_s, yaw_deg);
}

bool px4FlightHandler::stopOffboardVelocityNedControl()
{
	Offboard::Result ret = m_offboard->stop();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("stopOffboardVelocityNedControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("stopOffboardVelocityNedControl ok\n");

	return true;
}

bool px4FlightHandler::startOffboardVelocityBodyControl()
{
	// Send it once before starting offboard, otherwise it will be rejected.
	m_offboard->set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f});
	Offboard::Result ret = m_offboard->start();

	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("startOffboardVelocityBodyControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("startOffboardVelocityBodyControl ok\n");

	return true;
}

void px4FlightHandler::setVelocityBody(float forward_m_s, float right_m_s, float down_m_s, float yawspeed_deg_s)
{
	m_offboard->set_velocity_body({forward_m_s, right_m_s, down_m_s, yawspeed_deg_s});
	writeLogFile("setVelocityBody forward:%f, right:%f, down:%f, yawspeed:%f\n", forward_m_s, right_m_s, down_m_s, yawspeed_deg_s);
}

bool px4FlightHandler::stopOffboardVelocityBodyControl()
{
	Offboard::Result ret = m_offboard->stop();
	if(ret != Offboard::Result::SUCCESS)
	{
		writeLogFile("stopOffboardVelocityBodyControl error:%d\n", (int)ret);
		return false;
	}

	writeLogFile("stopOffboardVelocityBodyControl ok\n");

	return true;
}


px4FlightHandler flightHandler;
